/*
 * TestGoToPoint.java
 *
 * Created on 22 de agosto de 2007, 11:39 PM
 *
 * To change this template, choose Tools | Template Manager
 * and open the template in the editor.
 */

package Test;
import Behaviors.Arbitrators.VectorAdditionArbitrator;
import Behaviors.Schemas.Motor.MSFollowLeader;
import Behaviors.Schemas.Motor.MSGoToPoint;
import Behaviors.Schemas.Perception.PSDetectLeader;
import Controllers.BehaviorBasedController;
import Robot.*;
import Util.*;
/**
 *
 * @author jonathan
 */
public class TestFollowLeader {

    /**
     * Creates a new instance of TestGoToPoint
     */

    
    public static void process() {
        
        
       
    //***Definicion Robot 1*************************************
    Robot robotFollower=new Robot("192.168.1.51", 6665,"Follower",0); 
    Robot robotLeader=new Robot("192.168.1.50",6665,"Leader",0);
    
    Robot camera1=new Robot("127.0.0.1",6665,"camera0",0);
    Robot camera2=new Robot("127.0.0.1",6666,"camera1",0);
    
     FusionPosition2d positionLeader;
    try{
        positionLeader=new FusionPosition2d(camera1,robotLeader,"positionLeader",0);
    }
    catch(Exception e){
        e.printStackTrace();
        return;
    }
     
    FusionPosition2d positionFollower;
    try{
        positionFollower=new FusionPosition2d(camera2,robotFollower,"positionFollower",0);
    }
    catch(Exception e){
        e.printStackTrace();
        return;
    }
    
   
    
    /*RealPosition2d positionFollower;
    try{
        positionFollower=new RealPosition2d(robotFollower,"position1",0);
    }
    catch(Exception e){
        e.printStackTrace();
        return;
    }
    
    //***Definicion Robot 3*************************************
    
    RealPosition2d positionLeader;
    try{
        positionLeader=new RealPosition2d(robotLeader,"positionCenter",0);
    }
    catch(Exception e){
        e.printStackTrace();
        return;
    }
    //**********************************************************/

  
        robotFollower.runThreaded(10,0);
        robotLeader.runThreaded(10,0);
        camera1.runThreaded(10,0);
        camera2.runThreaded(10,0);

        //Formacion formacion=new Formacion();
        //positionLeader.setInitialPosition(new Pose2d(0.0f,0.0f,(float)Math.toRadians(90.0f)));
        //positionFollower.setInitialPosition(new Pose2d(0.0f,-1.0f,(float)Math.toRadians(90.0f)));
        Vector2d point=new Vector2d(0,0,Vector2d.MODE_XY);
        
        System.out.println("PointAngle="+Float.toString(point.getAngle()));
        
        PSDetectLeader detectLeader = new PSDetectLeader(positionLeader);
        MSFollowLeader followLeader = new MSFollowLeader("followLeader",positionFollower, detectLeader);
        MSGoToPoint goto2 = new MSGoToPoint("goto2",positionLeader,point,MSGoToPoint.MODE_EXPONENTIAL);
        
        //goto2.setMaxVel(0.01f);
        //followLeader.setGain(0.5f);
        

        //DetectFormation detectFormation=new DetectFormation(position1, positionCenter);
        //MaintainFormation mantainFormation=new MaintainFormation(detectFormation);
        VectorAdditionArbitrator arbitratorLeader=new VectorAdditionArbitrator(goto2,positionLeader, true);
        VectorAdditionArbitrator arbitratorFollower=new VectorAdditionArbitrator(followLeader,positionFollower, true);
        
        BehaviorBasedController controllerFollower=new BehaviorBasedController("follower",robotFollower,arbitratorFollower,10);
        BehaviorBasedController controllerGoto=new BehaviorBasedController("goto",robotLeader,arbitratorLeader,10);

        Thread hilo=new Thread(controllerFollower);
        Thread hilo2=new Thread(controllerGoto);
        
        
        hilo.start();
        hilo2.start();
       /*
        while(true)
        {
            controllerFollower.run();
            controllerGoto.run();
            try{
                
                Thread.sleep(100);
                
            }
            
            catch(Exception e){
                e.printStackTrace();
            }
            
        }*/
        
    
    }
    

}
